﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;

namespace MotionPlanning.MotionPlanningComponents.MotionPlanningSystems
{
    class System_RallyCar:System_SimplifiedPlanarPhysicalRobot
    {
        protected double m;
        protected double cf;
        protected double cr;
        protected double cv;
        protected double cfi;
        protected double a;
        protected double b;
        protected double I;

        override public string ToString()
        {
            return "\n\t<System_Type>Robotics_RallyCar</System_Type>" +
                    base.ToString();
        }

        override public double Distance2_Topological(double[] x1, double[] x2)
        {
            double diff_q1 = (x1[0] - x2[0]) * q_scale[0];
            double diff_q2 = (x1[1] - x2[1]) * q_scale[1];
            double diff_q3 = Math.Abs(x1[2] - x2[2]);
            while (diff_q3 > MotionPlanningUtils.PI_double)
                diff_q3 -= MotionPlanningUtils.PI_double;
            if (diff_q3 > Math.PI)
                diff_q3 = MotionPlanningUtils.PI_double - diff_q3;
            diff_q3 *= q_scale[2];

            double diff_q4 = (x1[3] - x2[3]) * q_scale[3];
            double diff_q5 = (x1[4] - x2[4]) * q_scale[4];
            double diff_q6 = (x1[5] - x2[5]) * q_scale[5];

            return diff_q1 * diff_q1 + diff_q2 * diff_q2 + diff_q3 * diff_q3 + diff_q4 * diff_q4 + diff_q5 * diff_q5 + diff_q6 * diff_q6;
        }

        override public bool LoadingSystemParameters(string fileName)
        {
            base.LoadingSystemParameters(fileName);

            TextReader tr = new StreamReader(fileName);
            if (tr == null)
                throw new Exception("input file cannot be opened.");
            string rawFile = tr.ReadToEnd();

            string[] separators = new string[] { "<m>", "</m>" };
            string[] entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.m = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<cf>", "</cf>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.cf = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<cr>", "</cr>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.cr = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<cv>", "</cv>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.cv = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<cfi>", "</cfi>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.cfi = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<a>", "</a>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.a = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<b>", "</b>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.b = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            separators = new string[] { "<I>", "</I>" };
            entries = rawFile.Split(separators, StringSplitOptions.None);
            if (entries.Length != 3) return false;
            this.I = MotionPlanningUtils.StringToDoubleArray(entries[1])[0];

            tr.Close();
            return true;
        }

        public void Qd_old(double[] q, double[] ctrl, ref double[] qd)
        {
            double thetta = q[2];
            double omega = q[3];
            double v = q[4];
            double s = q[5];
            double fi = ctrl[0];

            double ff = cf * (a * omega - v - fi * s);
            double fr = cr * (-b * omega - v);
            double CosT = Math.Cos(thetta);
            double SinT = Math.Sin(thetta);
            qd[0] = s * CosT + v * SinT;
            qd[1] = s * SinT - v * CosT;
            qd[2] = omega;
            qd[3] = (b * fr - a * ff) / I;
            qd[4] = (ff + fr) / m;
            qd[5] = ctrl[1] - Math.Sign(s) * (cv * Math.Abs(v) + cfi * Math.Abs(fi));
        }

        override public void Qd(double[] q, double[] ctrl, ref double[] qd)
        {
            double c1 = 1;
            double c2 = 1;
            double mu1 = -10;
            double mu2 = -10;
            double f1_max = 6;
            double f2_max = 6;
            double M = 1;
            double J = 1;
            //q[0] = x
            //q[1] = y
            //q[2] = thetta
            //q[3] = s
            //q[4] = v
            //q[6] = w
            //ctrl[0] = fi
            //ctrl[1] = lambda
            double x = q[0];
            double y = q[1];
            double tta = q[2];
            double s = q[3];
            double v = q[4];
            double w = q[5];
            double phi = ctrl[0];
            double lmd = ctrl[1];

            double cosT = Math.Cos(tta);
            double sinT = Math.Sin(tta);
            double cosP = Math.Cos(phi);
            double sinP = Math.Sin(phi);
            double cosTP = Math.Cos(tta + phi);
            double sinTP = Math.Sin(tta + phi);
            //_________________|___________________|_______________|____________|
            double mt_frnt_x =  s * cosT - v * sinT - w * c1 * sinT;
            double mt_frnt_y =  s * sinT + v * cosT + w * c1 * cosT;
            double mt_rear_x =  s * cosT - v * sinT + w * c2 * sinT - lmd * cosT;
            double mt_rear_y =  s * sinT + v * cosT - w * c2 * cosT - lmd * sinT;

            double mt_frnt_length = Math.Sqrt(mt_frnt_x * mt_frnt_x + mt_frnt_y * mt_frnt_y);
            double f1 = mt_frnt_length * mu1;
            if (Math.Abs(f1) > f1_max) f1 = f1_max * Math.Sign(f1);

            double mt_rear_length = Math.Sqrt(mt_rear_x * mt_rear_x + mt_rear_y * mt_rear_y);
            double f2 = mt_rear_length * mu2;
            if (Math.Abs(f2) > f2_max) f2 = f2_max * Math.Sign(f2);

            double f1_x = 0;
            double f1_y = 0;
            if(mt_frnt_length!=0)
            {
                f1_x = mt_frnt_x * mu1 * Math.Abs(f1 / mt_frnt_length);
                f1_y = mt_frnt_y * mu1 * Math.Abs(f1 / mt_frnt_length);
            }

            double f2_x = 0;
            double f2_y = 0;
            if (mt_rear_length != 0)
            {
                f2_x = mt_rear_x * mu2 * Math.Abs(f2 / mt_rear_length);
                f2_y = mt_rear_y * mu2 * Math.Abs(f2 / mt_rear_length);
            }

            double f1_sw = +f1_x * cosTP + f1_y * sinTP;
            double f1_vw = -f1_x * sinTP + f1_y * cosTP;

            double f2_s = +f2_x * cosT + f2_y * sinT;
            double f2_v = -f2_x * sinT + f2_y * cosT;

            double f1_s = -f1_vw * Math.Sin( phi );
            double f1_v = +f1_vw * Math.Cos( phi );

            double ds = (f1_s + f2_s) / M;
            double dv = (f1_v + f2_v) / M;
            double dw = (c1 * f1_v - c2 * f2_v) / J;

            double dx = s * cosT - v * sinT;
            double dy = s * sinT + v * cosT;
            double dtta = w;

            qd[0] = dx;
            qd[1] = dy;
            qd[2] = dtta;
            qd[3] = ds;
            qd[4] = dv;
            qd[5] = dw;
        }
    }
}
